package Lib3d.Physics;


import Lib3d.Vector3d;
import Lib3d.Animation.SimpleAnimationImpl;

import java.util.Vector;

/**
 * A constraint implementation that handles specific physics constainsts (those
 * of springs for instance).
 * 
 * @author Wassim Chegham
 * @version 0.1
 * @category Lib3d.Physics
 * 
 */
public class Constraint {
    public Mass mass1, mass2; // the two particles that are connected through
                              // this

    private final Spring spring;
    private final float r;

    // constraint

    public Constraint(final Spring spring, final Mass m1, final Mass m2) {
        this.mass1 = m1;
        this.mass2 = m2;
        this.spring = spring;
        this.r = Vector3d.sub(this.mass1.getLocation(), this.mass2.getLocation())
                .length();
    }

    public void satisfySpringConstraint2(final float dt) {
        // vector from p1 to p2
        final Vector3d p1_to_p2 = Vector3d.sub(this.mass1.getLocation(),
                this.mass2.getLocation());
        final float current_distance = p1_to_p2.length();
        final Vector3d correctionVector = Vector3d.mul(p1_to_p2, (1 - this.r
                / current_distance));
        final Vector3d correctionVectorHalf = Vector3d.mul(correctionVector, 1f);
        this.mass2.setOffsetLocation(correctionVectorHalf).solve(dt);
    }

    Mass getMass1() {
        return this.mass1;
    }

    Mass getMass2() {
        return this.mass2;
    }

    void satisfySpringConstraint(final float dt) {

        final Vector3d springVector = Vector3d.sub(this.mass1.getLocation(),
                this.mass2.getLocation());
        final float n = springVector.length();

        if (n != 0) {

            final float k = Spring.getSpringConstant();
            Vector3d force = new Vector3d(0, 0, 0);
            final Vector<Vector3d> f = new Vector<Vector3d>();

            // gravity
            f.add(new Vector3d(0, this.mass2.getMass() * Force.GRAVITY, 0));
            f.add(Vector3d.neg(SimpleAnimationImpl.getAnimationFunction()));

            // aplying formula:
            // force = (sVector / sNorme) * (sLength(t) - sLength(t-1)) * (-k);
            // TODO double check this formula!!
            force = Vector3d.mul(springVector, 1 / n);
            force = Vector3d.mul(force, this.spring.getDeltaLength());
            force = Vector3d.mul(force, -k);
            f.add(force);

            // adding the spring friction:
            // froce = -( v1 - v2 ) *friction

            // Vector3d b = Vector3d.sub(this.mass1.getVelocity(),
            // this.mass2.getVelocity());
            // b = Vector3d.neg(b);
            // b = Vector3d.mul(b, Spring.getDampingForce());
            // f.add(b);

            // aplying forces on mass2 only, coz mass1 is either fixed or has
            // already been solved!
            // XX[m1]------[m2]--- or ---[m1]------[m2]---

            this.mass2.applyForces(f).solve(dt);
        }

    }
}
